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INTRODUCTION 


A  substantial  need  exists  within  the  Armament  Research  and  Development 
Center  (ARDC)  for  a  readily  modifiable  and  computationally  efficient  flight  simu¬ 
lation  for  smart  munitions.  This  simulation  must  be  easily  adapted  to  varying 
designs  of  projectiles  undergoing  rapid  evolution.  Furthermore,  the  simulations 
must  be  highly  efficient  to  prevent  ARDC's  computer  facilities  from  being  over¬ 
whelmed  by  the  workload  associated  with  the  development  of  a  guided  projectile  or 
smart  munition. 

To  meet  this  need,  ARDC  Is  formulating  methods  and  developing  modular  soft¬ 
ware  for  simulating  autopilot  components  for  Incorporation  Into  Its  standard 
flight  simulation,  TRAJ.  An  Innovative  technique  was  developed  in  which  exact 
analytical  solutions  for  various  autopilot  functions  are  applied  In  piecewise 
fashion  within  the  more  complex  but  lower  frequency  simulation  of  the  projectile 
airframe,  which  is  solved  numerically  (ref  1).  The  extension  of  this  approach  to 
treat  two-axis  gimballed  gyroscopes  with  nutation  damping  is  documented  in  refer¬ 
ence  2.  The  module  presented  here  reproduces  gyro  nutation  as  well  as  precession 
without  the  extremely  small  Integration  time  steps  previously  required  for  a 
successful  numerical  Integration. 


DISCUSSION 


Gyroscopes  are  often  used  in  guided  projectiles  to  provide  inertial  sensing 
for  the  autopilot  or  a  stabilized  platform  for  a  seeker.  A  gyro  exhibits  both 
precession  and  nutation  when  torqued.  Since  nutation  is  not  ordinarily  desira¬ 
ble,  and  a  well  designed  gyroscope  should  have  sufficient  nutatlonal  damping  to 
make  the  nutation  negligible,  simulations  of  guided  projectiles  usually  neglect 
nutation  and  treat  gyroscopic  motion  as  a  simple  precession  rate  which  Is  propor¬ 
tional  to  the  applied  torque.  This  angular  rate  Is  then  numerically  Integrated 
to  produce  a  gyroscopic  angle. 

Attempts  to  treat  the  gyroscope  more  realistically  have  met  with  practical 
difficulties.  The  equations  for  a  gimballed  gyro  require  a  very  fine  Integration 
time  step  when  solved  by  numerical  integration  techniques.  Time  steps  can  be 
smaller  than  a  0.00001  second.  Clearly,  It  is  usually  not  practical  to  vise  such 
a  small  time  step  In  a  large  and  complex  flight  simulation. 

The  size  of  the  time  step  required  to  perform  a  numerical  Integration  is 
driven  downward  by  two  constraints:  (1)  the  driving  term  must  not  vary  signifi¬ 
cantly  during  the  time  step,  and  (2)  the  time  step  must  be  small  enough  to  pro¬ 
duce  a  stable  integration.  Reflection  will  convince  the  reader  that  the  second 
requirement  is  more  stringent  In  the  case  of  a  gyro  in  a  flight  simulation.  The 
time  constants  associated  with  the  airframe,  which  determine  the  rate  of  change 
of  the  driving  term,  are  an  Inherently  slow  process  compared  to  the  dynamics  of 
the  gyro.  Therefore,  stable  numerical  integration  is  a  stronger  driver  to  fine 
Integration  then  the  variation  of  the  driving  term.  The  proposed  piecewise  ana¬ 
lytical  approach  can  relax  the  time  step  size  by  orders  of  magnitude  by  eliminat¬ 
ing  the  second  constraint.  Such  a  solution  Is  exact  no  matter  how  large  the  time 


i 


step  as  long  as  the  driving  term  variation  is  negligible  during  that  interval. 
In  practice,  an  improvement  of  orders  of  magnitude  in  execution  speed  can  be 
achieved.  If  it  is  desirable  to  study  the  nutation  of  the  gyro,  the  time  step  of 
the  closed  form  solution  can  be  made  sufficiently  small  compared  to  the  nutation 
period  to  achieve  sufficient  resolution.  When  the  nutation  does  not  need  to  be 
resolved  from  the  precession,  the  size  of  the  time  step  can  be  determined  by  the 
requirements  imposed  by  the  rest  of  the  simulation. 


ANALYTIC  SOLUTION  TO  THE  EQUATIONS  OF  MOTION 


The  equations  of  motion  of  the  gimballed  gyro  are  (ref  3) 


Vl  +  fu)l  +  Hi°2  =  T1 


(la) 


where 


>2  u>  +  fu>2  -  HUj  =  T0  (lb) 


d  1  and  J’2  are  the  pitch  and  yaw  moments  of  Inertia,  respectively; 

f  Is  the  frictional  restraint  due  to  gimbal  bearing  friction; 

T |  and  T9  are  the  applied  pitch  and  yaw  torques,  respectively; 

'dj  and  ut  are  components  of  the  gyroscopic  angular  velocities;  and 

H  is  the  rotor,  angular  momentum,  2 tt  SJ,  where  S  is  the  rotor  spin  rate 
and  .T  Is  the  axial  moment  of  inertia. 

For  convenience  we  substitute 


,J|  -  X1 


=  X.  =  (i» 

2  k  7. 


(2) 


The  e<iuatlons  of  motion  then  become 
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These  two  differential  equations  can  be  decoupled  to  facilitate  obtaining  a 
solution  by  increasing  the  order  of  the  equations.  Assuming  Tj  to  he  constant, 
solving  equation  3  to  obtain  x^,  differentiating  vtt  and  solving  for  x^,  and  then 
substituting  these  results  in  equation  4  yields 


1  .  I 

• 

\r  X 

V  +  H2' 

fT{  -  HT2 

J1  J2 

x3 

_  J1J2 

x3  j  j 

J  J1J2 

(5) 


The  elimination  of  x^  has  resulted  In  an  equation  of  motion  In  the  form  of  a 
damped  harmonic  oscillator  with  a  constant  applied  force,  viz. 


mx  +  bx  +  kx  =  F 


(6) 


The  solution  to  equation  6  has  the  well-known  form 


F  —at 
x(t)  -  -  +  e 
m 


kj  sin  8t  +  k2  cos  Bt 


where  k^  and  k2  are  to  be  determined  by  the  initial  conditions  and 
a  =  b/2  m 
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By  comparing  equations  5  through  7,  an  expression  can  be  written  for  x-j 


=  x^(t)  =  k,.  +  e 


-at 


kj  sin  Bt  +  k2  cos  Bt 


Similarly,  the  following  expression  can  be  obtained  for  x^ 


-at 


k«  sin  Bt  +  k,  cos  Bt 
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w  =x.(t)  =k,  +e 


(10) 
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and  k^  through  k  ^  are  constants  of  Integration  to  be  determined  by  the  initial 
conditions.  Note  that  only  two  out  of  the  four  constants  of  integration  are 
independent  since  the  equations  are  second  order  only  because  of  the  artifice 
used  to  decouple  the  original  equations  of  motion.  If  t  *  0  is  set  in  equations 
9  and  10,  k2  and  k^  will  be  determined  in  terms  of  xjCO)  and  x^(0).  The  quanti¬ 
ties  kj  and  k-^  are  linearly  dependent  on  ^  and  k^.  To  see  this  explicitly, 
substitute  the  solutions  (equations  9  and  10)  into  the  original  equation  of 
motion  (equation  3)  to  obtain  the  expression 

ajjkj  +  8  JIk2  ~  ~  ^3  j  S*n 

+  aj  k  -  BJ  k  -  fk  -  Hk,  cos  St  =  0  (12) 

12  11  2  4 


Because  of  the  linear  independence  of  the  trigonometric  functions,  the  quantities 
In  brackets  must  each  vanish  identically.  This  condition  yields 
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f)k2  -  Hk^j 

f)kt  +  BJLk2J 

-  f)k4  -  H^k2 


(13) 


Therefore,  only  V-2  and  k^  are  specified  independently  and  can  now  be  evaluated 
from  the  initial  conditions  by  setting  t  *  0  in  equations  9  and  10. 


k2  =■  x^(0)  -  k5 

k.  -  x  (0)  -  k  (14) 
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This  completes  the  solution  to  equations  3  and  4;  however,  this  form  Is  not 
convenient.  The  solution  expressed  In  terms  of  the  gyroscopic  Euler 

angles  0  and  rather  than  the  angular  velocity  components  m  and  <o  is  preferred. 
Tiie  gryoscope  roll  angle  <j>  may  he  zero  with  no  loss  of  general  ity  since  a  two 
axis  gimhalled  gyroscope  only  has  two  degrees-of-f reedom .  Furtlie rmore ,  If  the 
gyroscope  motion  Is  integrated  stepwise  in  time,  discarding  the  old  coordinate 
svstem  at  the  end  of  each  integration  step  and  replacing  the  coordinates  with  a 
new  set  to  coincide  with  the  new  position  of  the  gyroscope,  small  angle  substi¬ 
tution  9  =  aj  ,  i*  =  to  can  be  made.  These  equations  can  then  be  integrated 
for  9  and  Xsing  standard  Integral  tables  to  obtain  the  final  results  given 
be  low. 
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0(  t )  =  k,-  +  e  kj  sin  Bt  +  k^  cos  Rt 


<Kt)  =  k,  +  e  i  k„  sin  Bt  +  k,  cos  Bt 


where 


kj  8  +  k2<i 


k  8  +  k ,  (» 

B2  =  \ - {-  (19) 

a  +  8 

A  brief  discussion  of  the  physical  significance  of  the  terms  in  the  solu¬ 

tions  for  0(t)  and  y(  t )  follows:  The  term  linear  in  t  with  coefficient  k^  or  k^ 
is  the  precession  that  ordinarily  dominates  gyroscopic  kinematics.  The^  terms 
lnvcjlvin}*  8  are  the  nutation  terms  with  frequency  f  =  3/2T,  and  [k^  +  k_]  and 
[k^  +■  k, ]  are  the  amplitudes  for  the  pitch  and  yaw  components.  For  consistency 
with  the  small  angle  approximation  used  to  obtain  equations  15  and  16,  these 

nutation  amplitudes  must  he  small. 

The  gyro  pitch  and  yaw  torques  are  cross-coupled  in  the  expressions  k^  and 
k^  whenever  f  ">  0.  In  the  subroutine  GYRO  (appendix),  noise  sources  and  error 
terms  are  Included  with  the  typical  treatment  given  by  gyroscope  manufacturers. 
Thev  are  for  Illustration  purposes  only.  Care  must  be  taken  with  each  gyroscope 
manufacturer  to  determine  which  of  these  terms  might  he  pertinent  and  to  verify 

that  they  are  not  merely  a  phenomenological  way  of  including  in  simpler  gyro 
models  the  effects  that  flow  naturally  from  this  more  complete  treatment.  For 
example,  a  typical  term  is  torquer  cross  coupling.  Such  an  effect  can  arise  in 
part  because  of  the  nonorthogonality  of  the  pitch  and  yaw  components  of  the  mag¬ 
netic  fields  of  the  torquer,  especially  when  the  gimbal  angles  are  not  zero. 

However,  torque  is  also  cross  coupled  in  the  expressions  k^  and  k^  whenever 
f  >  D.  Such  effects  should  not  he  accovinted  for  twice. 

The  computer  realization  of  the  algorithm  is  as  follows:  At  the  beginning 
of  each  integration  time  step,  a  coordinate  system  is  fixed  that  momentarily 
coincides  with  the  gyroscope  axes  at  the  beginning  of  the  interval .  The  gyro¬ 
scopic  motion  is  integrated  in  this  artificially  fixed  inertial  coordinate  system 
with  the  angles  initially  zero.  Therefore,  if  the  integration  time  step  is  not 
too  large,  the  small  angle  approximation  is  maintained.  At  the  end  of  the  time 
step,  the  gyro  gimbal  angles,  gyro  Euler  angles,  and  Euler  rotation  matrices  can 
he  reconst ructed  by  using  the  gyroscopic  rotation  matrix  at  the  beginning  of  the 
integration  time  step  and  the  projectile  body  rotation  matrix  at  the  end  of  the 
integration  time  step. 


CONCLUSIONS 

This  piecewise  analytic  solution  to  a  two-axis  glmhalled  gyroscope  results 
in  considerable  savings  in  computer  run  time. 
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SYMBOLS 


Algebraic  Symbols 


f 

H 

.1 


J 


l  * 


s 


Tl.  T2 


V  w2 


Frictional 
Gyro  rotor 
Gyro  axial 
Gyro  pitch 
Gyro  rotor 


restraint  due  to  glinbal  bearing  friction 
angular  momentum,  2  it  s.I ,  (slug-ft2/s) 
moment  of  Inertia  (slug-ft~) 
and  yaw  moments  of  Inertia,  respectively 
spin  rate  (Hertz) 


( slug-f t^/ s) 


(slug-f t2) 


?  ON 

Applied  gyro  pitch  and  yaw  torques,  respectively  (slug-ft  /s4; 

Pitch  and  yaw  components  of  the  gyroscopic  angular  velocities, 
respectively  (rad/s) 


FORTRAN  Symbols 


In  this  simulation,  the  "Inertial  small  angle  frame”  Is  defined  as  a  set  of 
nonrotating  Inertial  coordinates  that  momentarily  coincides  with  the  gyro  axes  at 
the  beginning  of  each  Integration  time  step.  This  coordinate  frame  Is  abbrevi¬ 
ated  as  ISAF. 


F 

GERROR 

GIMBAL 

G1MBY 

CIMBZ 

G.IO 

G.T  1  ,  GJ2 
GP 


GSPIN 


H 


Frictional  restraint  due  to  gyro  pitch/yaw  gimbal-bear ing  fric¬ 
tion  (slug-ft2/s) 

Matrix  of  gyro  error  sources  (various  units) 

Glmbal  limit  angle  (radians) 

Pitch  gyro  gimbal  angle  (radians) 

Yaw  gyro  gimbal  angle  (radians) 

2 

Gyro  spin-axis  moment  of  inertia  (slug-ft  ) 

9 

Gyro  pitch  and  yaw  axes  moments  of  inertia  (slug-ft“) 

Components  of  a  unit  vector  coinciding  with  the  gyro  spin  axis 
and  resolved  In  projectile  body  axes;  used  internally  only. 

Gyro  spin  rate  (Hertz).  GJO  and  GSPIN  are  used  only  to 
calculate  H. 

Gyro  rotor  angular  momentum  (slug-f t2/s) 


IGLF. 


ISAE 

RSI 

RS  [  I) 

RSIOCM 

RSIDMX 

ROTC 

ROTKG 

TPEl.TA 

THETA 

THKTAO 

THTDCM 

THTOMX 


Gimbal  limits  exceeded  indicator:  1  =  limits  exceeded 

0  =  not  exceeded 

Small  angle  exceeded  indicator:  1  =  limits  exceeded 

0  =  not  exceeded 

Gyro  yaw  angle  in  ISAF  (radians) 

Time  derivative  of  PSI  (rad/s) 

Commanded  gyro  yaw  slew  rate  (rad/s) 

Maximum  commanded  gyro  yaw  rate  (rad/s) 

Value  of  the  matrix  ROTEG  at  the  beginning  of  each  time  step 
Gyro  Euler  angle  rotation  matrix  (earth  to  gyro) 

Integration  time  step  (seconds) 

Gyro  pitch  angle  in  the  ISAE  (radians) 

Time  derivative  of  THETA  (rad/s) 

Commanded  gyro  pitch  slew  rate  (rad/s) 

Maximum  commanded  gyro  pitch  rate  (rad/s) 


COM PUT HR  PROGRAMS 


The  formalism  described  In  this  report  was  Implemented  In  FORTRAN  77  and 
comprises  three  parts:  (l)  a  main  program  MAINP  that  Is  a  surrogate  for  a  six 

degree-of-f reedom  simulation  In  which  the  gyro  model  Is  to  be  embedded,  (2)  an 
Initialization  subroutine  INITG,  and  (3)  the  gyro  model  subroutine  (TYRO.  Subrou¬ 
tine  GYRO  Is  suitable  for  use  as  It  stands.  INITG  Is  meant  to  serve  as  a  guide 
to  creating  the  proper  Initialization  in  the  user's  own  simulation.  MAINP  Is 
meant  to  be  completely  replaced  by  the  user's  six  deg ree-o f-f reedom  simulation 
and  Is  only  used  here  as  a  driver  for  testing  or  demonstrating  subroutine  GYRO. 

A  sample  run  was  made  with  100-hertz  gyro  spin  rate  and  5  slug-ft^/s  fric¬ 
tional  restraint  for  the  two  input  data  records.  In  addition  to  the  tabular 
output,  plots  (figs  A-l  and  A-2)  show  the  rapid  nutation  and  the  general  slow 
drift  which  is  the  precession. 

The  program  GYRO  is  self-documented  with  comment  statements  and  can  be  used 
as  It  stands  but  the  user  should  examine  the  comments  so  that  interfacing  re¬ 
quirements  and  the  definitions  of  all  the  variables  and  constants  are  understood. 
COMMON/SIXDOF/  Is  meant  to  be  replaced  by  whatever  COMMON  BLOCK  is  required  in 
the  user's  own  six  degree-of-f reedom  simulation  to  communicate  the  values  of 
these  parameters  needed  by  subroutine  GYRO.  Note  that  the  resulting  rotation 
matrix  is  regularly  renormalized  to  restore  its  unitary  property  at  the  end  of 
subroutine  GYRO. 
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PROGRAM  GYMAIN 


MAIN  PROGRAM  TO  DRIVE  GYRO  SUBROUTINE. 

FOR  TEST  PURPOSES  ONLY. 

DIMENSION  ROTX (3,3) ,ROTY(3,3) ,ROTZ(3,3) ,ROTNEW(3,3) 

COMMON/GYROD/  GJ1,GJ2,F,GIMBAL/THTDMX,PSIDMX,GERR0R(6,2) 

+  THETAD,PSID,ROTEG (3,3) , THETA, PSI ,  THTDCM , PS I DCM , H , 

+  GIMBY,GIMBZ,  GJ0,GSPIN,  IGLE, ISAE 
COMMON /SI XDOF/ROTEB (3,3) ,VD,WD,G,PI 

INITIAL  DATA 

OPEN  (UNIT=7,FILE=' INPUT') 

REWIND  7 

OPEN  (UNIT=6 ,FILE= 'OUTPUT ' ) 

REWIND  6 
VD  =0.0 
WD  =0.0 
GJ0=3.0 
GJl=2. 25 
GJ2=2 . 00 
GIMBAL=. 4363323 
THTDMX=. 1745329 
PSIDMX=. 1745329 

1 


G=32. 1725 


PI=3. 1415927 


DO  10  J=l,2 
DO  10  K=l,6 
GERROR (K, J) =0.0 
10  CONTINUE 

WRITE (6, 1007) 

READ(7,1004)GSPIN 
READ (7, 1004) F 
DO  1  1=1,3 
DO  1  J=l, 3 
EL=0 . 

IF(I.EQ.J)  EL=1 . 

ROTX  (I  ,J)  =EL 
ROTY (I , J) =EL 
ROTZ (I ,J) =EL 
ROTEB ( I , J) =EL 
ROTNEW ( I , J ) =EL 
1  ROTEG ( I , J ) =EL 
CALL  INITG (TSTEP) 
WRITE (6, 1005)  GSPIN,F 
EL=. 1745329*TSTEP 
ROTY ( 3 , 1 ) =2 . 3*EL 
ROTY (1,3) =-2.3*EL 
ROTZ (1 ,2) =1 . 8*EL 


ROTZ (2,1) =-1.8*EL 


GIMBY=0 


fc.>  Ol  i.k  -1  -  >k 


GIMBZ=0. 

CALL  GYRO (T) 

CALL  GYRO  WITH  ZERO  ARGUMENT  AT  UNCAGING  TO  INITIALIZE. 

WRITE {6, 1001) 

THETAD=ROTY (3,1) /TSTEP 

PS ID  =ROTZ (1,2) /TSTEP 

THETA  =.0 

PSI  =  .Q 

DO  3  KK=0,800 

THTDCM  =-.19 

IF  (KK.GE.40)  THTDCM=0 . 0 

PSIDCM=.013 

CALL  GYRO (TSTEP) 

IF  (IGLE.EQ.l)  STOP 
IF  (ISAE.EQ.l)  STOP 
IF  (KK.GE.60)  PSIDCM=-.151 
T=T+TSTEP 

WRI T E ( 6 , 1 0 0 0 ) KK , T , THET A , PS I , THTDCM , PS I DCM 
DO  2  1=1,3 
DO  2  N=1 , 3 
ROTNEW ( I , N ) =0 . 

DO  2  J=l, 3 
DO  2  K=l, 3 
DO  2  L=l,3 

2  ROTNEW(I,N)=ROTNEW(I,N)+ROTZ(I,J)*ROTY(J,K)*ROTX(K,L)*ROTEB  (L,N) 


-v  •  .y. 


DO  4  1=1,3 


DO  4  K=1 , 3 

4  ROTEB ( I ,  K) =ROTNEW ( I  ,K) 


3  CONTINUE 


1000  FORMAT (IX, 16, 3X ,F10 . 6 , 2F14.6 , 3X , 2F10. 4 ) 

1001  FORMAT (16X, 4HTIME, 9X, 5HTHETA, 11X, 3HPSI , 7X,6HTHTDCM,4X,6HPSIDCM) 

1004  FORMAT (F20. 10) 

1005  FORMAT (11H  GSPIN  IS  ,F10.2,7H  HERTZ.  ,/,5H  F  IS,F10.5, 

$  16H  SLUG*FT**2/SEC.  ,//) 

1006  FORMAT (8H  TIME  IS,F15.8,5H  SEC.  ,/) 

1007  FORMAT (1H1) 


SUBROUTINE  INITG (TSTEP) 


GYRO  INITIALIZATION  ROUTINE 


COMMON/GYROD/  GJl,GJ2,F,GIMBAL,THTDMX,PSIDMX,GERROR (6, 2) , 
+  THETAD,PSID,ROTEG(3,3) , THETA, PS I,  THTDCM,PSIDCM,H, 

+  GIMBY,GIMBZ,  GJ0, GSPIN,  IGLE, ISAE 
COMMON/S I XDOF/ROTEB (3,3) ,VD,WD,G,PI 


H  =  2.*PI*GSPIN*GJ0 

ALPHA  =  (F/2. ) * (1./GJ1  +  1./GJ2) 

BETA  =  SQRT (  (F*F+H*H) / (GJ1*GJ2)  -  ALPHA**2  ) 

NUTATION  FREQUENCY  IS  USED  ONLY  INFORMATIVELY  AND  IN  THE 
SELECTION  OF  A  TIME  STEP  TO  PERMIT  RESOLUTION  OF  THE 


NUTATION.  GNUTAT  IS  IN  HERTZ  (CPS) . 


GNUTAT  =  BETA/ (2.*PI) 

WRITE (6, 1000)  GNUTAT 

SELECTING  OUTPUT  RATE  TO  RESOLVE  NUTATION. 

TSTEP  =  GNUTAT*20. 

TSTEP  =  1. /FLOAT (IFIX (TSTEP) ) 

WRITE (6, 1001)  TSTEP 
RETURN 

1000  FORMAT (3 OH  GYRO  NUTATION  FREQUENCY  IS  ,Fl0.5  ,7H  HERTZ.  ) 

1001  FORMAT (14H  TIME  STEP  IS  ,F10.5  ,5H  SEC.  ) 

END 

SUBROUTINE  GYRO (TDELTA) 

THIS  ROUTINE  MOVES  THE  GYRO  GIMBAL  PICKOFF  ANGLES  GIMBY  &  GIMBZ 
AND  THE  GYRO  ROTATION  MATRIX  ROTEG  ONE  TIME  STEP  TDELTA 
FORWARD. 

DIMENSION  ROT (3,3) ,ROTC(3,3) ,GP(3) ,X(13) 

COMMON./GYROD/  GJ1,GJ2, F, GIMBAL, THTDMX, PSIDMX,GERROR (6,2) , 

+  THETAD , PS  I D , ROTEG (3,3) , THETA , PS  I ,  THTDCM , PS I DCM , H , 

+  GIMBY, GIMBZ,  GJ0,GSPIN,  IGLE, ISAE 

COTt10N/SIXDOF/ROTEB(3,3) ,VD,WD,G,PI 

INITIALIZE  ELSEWHERE  ONCE  AND  FOR  ALL:  GJ1 ,GJ2,F, GIMBAL, THTDMX, 


PS  I DMX , GERROR (6,2) 


INITIALIZE  ELSEWHERE  AT  UNCAGING:  THETAD,PSID  TO  PROJECTILE  BODY 
RATES;  ROT EG  TO  ROTEB+OFFSET  ERROR;  THETA, PSI  TO  ZERO+ 

OFFSETS. 

INPUT  ARGUMENTS:  TDELTA,THTDCM,PSIDCM,H,VD,WD. 

OUTPUT  ARGUMENTS:  GIMBY,GIMBZ,ROTEG, IGLE, ISAE. 

GSPIN  AND  GJO  ARE  ONLY  NEEDED  TO  CALCULATE  H.  TO  MODEL  GYRO  SPIN 
DOWN,  ASSIGN  ELSEWHERE  AS  INPUT  TO  THIS  ROUTINE  A  VARYING 
VALUE  OF  GSPIN.  H  IS  THEN  CALCULATED  IN  THIS  ROUTINE. 

IN  THIS  PROGRAM,  THE  'INERTIAL  SMALL  ANGLE  FRAME'  IS  DEFINED 
AS  A  SET  OF  NONROTATING  INERTIAL  COORDINATES  MOMENTARILY 
COINCIDING  WITH  THE  GYRO  AXES  AT  T=0.  IT  WILL  BE  ABBREVIATED 
AS  ' ISAF' . 

TDELTA  INTEGRATION  TIME  STEP  (SEC) . 

THETA  GYRO  PITCH  ANGLE  IN  THE  ISAF  (RAD)  -INFINITESIMAL  ANGLE 

PSI  GYRO  YAW  ANGLE  IN  ISAF  (RAD)  -INFINITESIMAL  ANGLE. 

THETAD  TIME  DERIVATIVE  OF  THETA  (RAD/SEC) . 

PSID  TIME  DERIVATIVE  OF  PSI  (RAD/SEC) . 

GJ1,GJ2  GYRO  PITCH  AND  YAW  AXIS  MOMENTS  OF  INERTIA  (SLUG*FT**2) 
F  FRICTIONAL  RESTRAINT  DUE  TO  PITCH/YAW  GIMBAL-BEARING 

FRICTION.  (SLUG*FT**2/SEC) . 


H 


GYRO  ROTOR  ANGULAR  MOMENTUM  (SLUG*FT**2/SEC) 


C  PSIDCM  COMMANDED  GYRO  YAW  SLEW  RATE  (RAD/SEC) . 

C  THTDCM  COMMANDED  GYRO  PITCH  SLEW  RATE  (RAD/SEC) . 

C  GIMBAL  GIMBAL  LIMIT  ANGLE  (RAD) . 

C  THTDMX  MAXIMUM  COMMANDED  GYRO  PITCH  RATE  (IMPLIED  TORQUER 

C  LIMIT) .  (RAD/SEC) 

C  PSIDMX  MAXIMUM  COMMANDED  GYRO  YAW  RATE.  (RAD/SEC) . 

C  GERROR  MATRIX  OF  GYRO  ERROR  SOURCES  (VARIOUS  UNITS) .  SEE  BELOW. 

C  ROTEG  GYRO  EULER  ANGLE  ROTATION  MATRIX  (EARTH  TO  GYRO) . 

C  ROTC  ROTEG  AT  BEGINNING  OF  TIME  STEP. 

C  GP  COMPONENTS  OF  A  UNIT  VECTOR  COINCIDING  WITH  THE  GYRO 

C  SPIN  AXIS  AND  RESOLVED  IN  PROJECTILE  BODY  AXES. 

C  (USED  INTERNALLY  FOR  CALCULATIONS  IN  THIS  ROUTINE  ONLY.) 

C  GIMBY  PITCH  GYRO  GIMBAL  ANGLE  (ANGLE  GYRO  MAKES  WITH  RESPECT 

C  TO  PROJECTILE  BODY) .  (RAD) 

C  GIMBZ  YAW  GYRO  GIMBAL  ANGLE  (RAD) . 

C  IGLE  GIMBAL  LIMITS  EXCEEDED  INDICATOR  (1  =  LIMITS  EXCEEDED, 

C  0  =  NOT  EXCEEDED) . 

C  ISAE  SMALL  ANGLE  EXCEEDED  INDICATOR  (1  =  LIMITS  EXCEEDED, 

C  0  =  NOT  EXCEEDED) . 

C  CURRENT  VALUE  OF  .01  ASSURES  AT  LEAST  4  DIGITS. 

C 

C  GJO  GYRO  SPIN-AXIS  MOMENT  OF  INERTIAL  (SLUG*FT**2) . 

C  GSPIN  GYRO  SPIN  RATE  (CYCLES/SEC) . 

C  GJO  AND  GSPIN  USED  ONLY  TO  CALCULATE  H. 

C 

C  VARIABLES  FROM  THE  SIXDOF  PROGRAM  (SEE  COMMON/S I XDOF/. . ) 

C  ROTEB  PROJECTILE  EULER  ROTATION  MATRIX  (EARTH  TO  BODY) . 


VD,WD  DERIVATIVES  OF  V  AND  U,  PROJECTILE  VELOCITY  COMPONENTS 
IN  BODY  AXES  (FT/SEC) . 

G  ACCELERATION  OF  GRAVITY  (FT/SEC* *2) . 

INITIALIZE  INDICATORS 
IGLE  =  0 
ISAE  =  0 

IF  ARGUMENT  <  OR  =  0,  THEN  JUST  INITIALIZE  ROTC. 

IF  (TDELTA.LE.O.)  GO  TO  10 
TT1  =  PSIDCM 
TT2  =  THTDCM 

SLEW  RATE  LIMIT  (IMPLIED  TORQUER  LIMIT). 

IF  (ABS(TTl) .GT.THTDMX)  TTl=SIGN (THTDMX,TT1) 

IF  (ABS(TT2) .GT.PSIDMX)  TT2=SIGN (PSIDMX, TT2) 

GYRO  ERROR  SOURCES  -  STATIC  DRIFT  (RAD/ SEC) . 

TT1  =  TT1  +  GERROR (1,1) 

TT2  =  TT2  +  GERROR (1,2) 

IN-PLANE  AND  CROSS-PLANE  AUTO  ERECTION  (1/SEC) . 

TT1  =  TT1  +  GERROR (2, 1)*(-GIMBY)  +  GERROR (3 , 1 )* (-GIMBZ) 

TT2  =  TT2  +  GERROR (2,2) * (-GIMBZ)  -  GERROR ( 3, 2) * (-GIMBY) 

G-SENSITIVE  DRIFT  (RAD/SEC) . 

IF  (ABS(WD) .GT.G)  TT1  =  TT1  +  (WD/G  -  SIGN (1 . ,WD) ) *GERROR (4 , 1) 
IF  (ABS(VD) .GT.G)  TT2  =  TT2  +  (VD/G  -  SIGN (1 . ,VD) ) *GERROR (4 , 2) 
TORQUER  CROSS-COUPLING.  DO  NOT  INCLUDE  CONTRIBUTIONS  ALREADY 
ACCOUNTED  FOR  BY  THE  EXPRESSIONS  X(8)  AND  X(9)  (DIMENTIONLESS). 
TT1  =  TT1  +  PS I DCM*GERROR (5,1) 


TT2  =  TT2  -  THTDCM*GERROR (5,2) 

COULOMB  FRICTION  (RAD/SEC) . 

TT1  =  TT1  -  SIGN(l.,PSID)*GERROR(6,l) 

TT2  =  TT2  -  SIGN (1. ,THTD) *GERROR (6,2) 

APPLIED  TORQUES. 

H  =  2.*PI*GSPIN*GJ0 
Tl  =  H*TTI 
T2  =  -H*TT2 

ALPHA  =  (F/2. ) * (l./GJl  +  1./GJ2) 

BETA  =  SQRT (  (F*F+H*H) / (GJ1*GJ2)  -  ALPHA* ALPHA  ) 
X  (1)  =  F*F+H*H 

X  (2)  =  BETA*GJ1 

X (3)  =  ALPHA*GJ1-F 

X (4)  =  ALPHA**2  +  BETA* *2 

X  (5)  =  S I N ( BETA * TDELTA ) 

X  (6)  =  COS (BETA*TDELTA) 

X (7 )  =  EXP (-ALPHA*TDELTA) 

X  (8)  =  (F*T1-H*T2)/X(1) 

X (9)  =  (H*T1+F*T2)/X(1) 

GK2  =  THETAD-X (8) 

GK4  =  PSID-X (9) 

GKl  =  (X (3) *GK2-H*GK4) /X (2) 

GK3  =  (X (3) *GK1+X (2) *GK2)/H 

X  (10)  =  (GK1*BETA+GK2*ALPHA)  A(4) 

X  ( 1 1 )  =  (GK1*ALPHA-GK2*BETA) A (4) 

X (12)  =  (GK3*BETA+GK4*ALPHA) A (4 ) 

X  (13)  =  (GK3*ALPHA-GK4*BErA) A (4) 


THETA  =  THETA+X  (8)  *TDELTA+X  ( 10)  -X  (7 )  *  (X  (11 )  *X  (5)  +X  (10)  *X  (6) ) 


PSI  =  PS  I +X  ( 9 )  *TDELTA  +X  ( 1 2 )  -X  ( 7 )  *  (X  ( 1 3 )  *X  ( 5 )  +X  ( 1 2 )  *X  (6 ) ) 
THETAD  =  X (8) +X  (7) * (GKl*X (5) +GK2*X (6) ) 

PSID  =  X (9) +X (7) * (GK3*X (5) +GK4*X (6) ) 

TEST  IF  SMALL  ANGLE  APPROXIMATION  IS  VIOLATED. 

THIS  TEST  (.01)  ASSURES  AT  LEAST  4  DIGITS  OF  ACCURACY. 

AMNUTY  =  (GK1*GK1  +  GK2*GK2)A(4) 

AMNUTZ  =  (GK3*GK3  +  GK4*GK4) /X (4 ) 

AMNUTY  =  SQRT (AMNUTY) 

AMNUTZ  =  SQRT (AMNUTZ) 

IF  (AMNUTY. GT.  .01  .OR.  AMNUTZ. GT.  .01)  ISAE=1 
IF  (ABS (THETA) .GT.  .01  .OR.  ABS(PSI).GT.  .01)  ISAE=1 

ROT (1,1)  =  1. 

ROT  (1,2)  =  PSI 
ROT (1,3)  =  -THETA 
ROT (2,1)  =  -PSI 
ROT (2,2)  =  1. 

ROT (2,3)  =  0. 

ROT (3,1)  =  THETA 
ROT  (3,2)  =  0. 

ROT (3,3)  =  1. 

ROT  IS  THE  INFINITESIMAL  EULER  ANGLE  ROTATION  GOING  FROM  ' ISAF 
AXES  TO  CURRENT  GYRO  AXES  AT  END  OF  INTEGRATION. 

DO  2  J=1 , 3 


DO  2  K=l, 3 


ROTEG (J,K) =0. 


DO  2  L=1 , 3 

ROTEG  (J ,  K)  =ROTEG  (J  ,K)  +ROT  (J,L)  *ROTC  (L,K) 

2  CONTINUE 
DO  3  J=1 , 3 
GP (J) =0. 

DO  3  K=I,3 

3  GP(J)=ROTEB(J,K)*ROTEG<l,K)+GP(J) 

TFMP=S0RT (GP (1) *GP (1) +GP (2) *GP (2) ) 

GIMBY=ATAN2 (+GP (3) #TEMP) 

GIMBZ=ATAN2 (-GP (2) ,GP (1) ) 

FOLLOWING  IS  APPROPRIATE  TEST  IF  TOTAL  GIMBAL  ANGLE  IS  THE 
LIMITING  FACTOR. 

1 F (GP (1 ) .GT.COS (GIMBAL) )  GOTO  10 

IN  SOME  DESIGNS,  THE  INDIVIDUAL  GIMBAL  ANGLES  MAY  BE 
THE  LIMITING  FACTOR  AND  SHOULD  BE  TESTED  SEPARATELY. 

IF  (GIMBY.CT.COS (GIMBAL) .AND. GIMBZ. GT.COS (GIMBAL))  GOTO  10 


WRITE (6, 1000) 

IGLE=1 

RETURN 


RENORMALIZING  MATRIX  PERIODICALLY  TO  RESTORE  UN I TAR I TY . 
10  IF  (NCT. LT. O.OR. NCT.GT. 20 )  NCT=0 


G1MBY=0.0 


GIMBZ=0 • 0 
NCT=NCT+1 

IF  (NCT.LT.20)  GO  TO  13 
REN  =  0.0 
DO  11  1=1,3 
DO  11  K=l, 3 

11  REN  =  REN  +  ROTEG ( I , K) *ROTEG (I ,K) 

REN  =  SQRT (REN)/3. 

DO  12  1=1,3 
DO  12  J=l, 3 

12  ROTEG ( I , J) =ROTEG ( I , J) /REN 

13  IX)  14  J  =  l,3 
DO  14  K=1 , 3 

14  ROTC(J,K)=ROTEG(J,K) 

r> 

w 

C  THE  FOLLOWING  WILL  CALCULATE  THE  GYRO  ROTATION  MATRIX 
C  THAT  ROTATES  VECTORS  FROM  THE  PROJECTILE  BODY  AXES  TO 

C  THE  GYRO  ACES.  IF  NEEDED,  REMOVE  'C'  AND  INSERT  'ROTBG 

C  INTO  COMMON  BLOCK  'GYRO'. 

C 

C  DO  15  1=1,3 

C  DO  15  J=l, 3 

C  ROTBG (I, J) =0.0 

C  DO  15  K=1 , 3 

C  15  ROTBG  ( I ,  J)  =ROTBG  ( I ,  J ,  +ROTEB  ( I  ,K)  *ROTEG  (J  ,K) 


RETURN 
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